#include "m5_atom_s3r.h"

#include "stdbool.h"
#include "string.h"
#include "esp_log.h"
#include "esp_err.h"

#include "freertos/FreeRTOS.h"    
#include "freertos/task.h"
#include "freertos/semphr.h"

#include "bmi270.h"

static bool bmi270_initialized = false;
static bmi270_handle_t bmi270_handle = NULL;
static uint8_t enable_sensors[10] = {0};  
static uint8_t enable_sensor_count = 0;

static SemaphoreHandle_t imu_mutex;

//更新传感器数据用变量
static uint8_t sensor_feature_update[10] = {0};
static uint8_t feature_update_count = 0,sensor_update_count = 0;
struct bmi2_feat_sensor_data feature_data[10];
static struct bmi2_sens_data sensor_data;

static imu_data_t imu_data;
static imu_motion_data_t imu_motion_data;

static TaskHandle_t imu_task_handle = NULL;
void IMU_Task(void *pvParameters)
{
    while(1)
    {
        xSemaphoreTake(imu_mutex,portMAX_DELAY);
        //更新环节
        if(sensor_update_count>0)
            bmi2_get_sensor_data(&sensor_data, bmi270_handle);
        //特性更新环节
        if(feature_update_count>0)
        {
            int8_t feature_result = bmi270_get_feature_data(feature_data,feature_update_count,bmi270_handle);
            if(feature_result != BMI2_OK)
            {
                ESP_LOGE("IMU_TASK","BMI270 get feature data. errorcode:%d",feature_result);
            }
        }

        //处理环节
        imu_data.acc.x = (float)sensor_data.acc.x/BMI2_ACC_FOC_8G_REF;
        imu_data.acc.y =  (float)sensor_data.acc.y/BMI2_ACC_FOC_8G_REF;
        imu_data.acc.z =  (float)sensor_data.acc.z/BMI2_ACC_FOC_8G_REF;
        imu_data.gyro.x =  (float)sensor_data.gyr.x/BMI2_GYRO_FOC_2000_DPS_REF;
        imu_data.gyro.y =  (float)sensor_data.gyr.y/BMI2_GYRO_FOC_2000_DPS_REF;
        imu_data.gyro.z =  (float)sensor_data.gyr.z/BMI2_GYRO_FOC_2000_DPS_REF;
        

        for(int i=0;i<feature_update_count;i++)
        {
            switch(feature_data[i].type)
            {
                case BMI2_SIG_MOTION:
                case BMI2_ANY_MOTION:
                case BMI2_NO_MOTION:
                    imu_motion_data.activity = feature_data[i].sens_data.activity_output;
                    break;
                case BMI2_STEP_COUNTER:
                    imu_motion_data.step_counter = feature_data[i].sens_data.step_counter_output;
                    break;
                case BMI2_WRIST_GESTURE:
                    imu_motion_data.wrist_gesture = feature_data[i].sens_data.wrist_gesture_output;
                    break;
                default:
                    break;
            }
        }
        
        xSemaphoreGive(imu_mutex);
        vTaskDelay(10/portTICK_PERIOD_MS);
    }
}
/**
 * @brief BMI270 sensor 
 * 
 */


// 初始化 BMI270 传感器
esp_err_t imu_init(void)
{
    if (bmi270_initialized) {
        return ESP_OK;
    }
    
    imu_mutex = xSemaphoreCreateMutex();

    // 创建并初始化传感器
    bmi270_i2c_config_t i2c_config = {
        .i2c_handle = bsp_i2c_get_handle(),
        .i2c_device_config = {
            .dev_addr_length = I2C_ADDR_BIT_LEN_7,
            .device_address = BMI270_I2C_ADDRESS,
            .scl_speed_hz = 400 * 1000,
            .scl_wait_us = 0,
            .flags = {
                .disable_ack_check = 0,
            },
        },
    };

    esp_err_t ret = bmi270_sensor_create(&i2c_config, &bmi270_handle);
    if (ret != ESP_OK) {
        ESP_LOGE("BMI270", "Sensor creation failed");
        return ret;
    }

    int8_t init_result = bmi270_init(bmi270_handle);
    if (init_result != 0) {
        ESP_LOGE("BMI270", "Sensor initialization failed");
        return ESP_FAIL;
    }

    ESP_LOGI("BMI270", "Sensor initialized successfully");

    xTaskCreate(IMU_Task, "IMU_Task", 4096, NULL, 4, &imu_task_handle);
    ESP_LOGI("BMI270", "IMU Task created");

    bmi270_initialized = true;
    return ESP_OK;
}

// 删除传感器对象并释放资源
esp_err_t imu_deinit(void)
{
    if (!bmi270_initialized) {
        ESP_LOGE("BMI270", "BMI270 not initialized");
        return ESP_ERR_INVALID_STATE;
    }
    xSemaphoreTake(imu_mutex,portMAX_DELAY);

    vTaskDelete(imu_task_handle);
    imu_task_handle = NULL;
    ESP_LOGI("BMI270", "IMU Task deleted");


    esp_err_t del_result = bmi270_sensor_del(bmi270_handle);
    if (del_result != ESP_OK) {
        ESP_LOGE("BMI270", "Failed to delete sensor object");
        return ESP_FAIL;
    }

    ESP_LOGI("BMI270", "Sensor object deleted successfully");

    vSemaphoreDelete(imu_mutex);

    bmi270_initialized = false;
    return ESP_OK;
}

imu_data_t *imu_get_data_ptr(void)
{
    if (!bmi270_initialized) {
        ESP_LOGE("BMI270", "BMI270 not initialized, cannot get data ptr");
        return NULL;
    }
    return &imu_data;
}

imu_motion_data_t *imu_get_motion_ptr(void)
{
    if (!bmi270_initialized) {
        ESP_LOGE("BMI270", "BMI270 not initialized, cannot get motion ptr");
        return NULL;
    }
    return &imu_motion_data;
}

// 启用传感器和特性
esp_err_t imu_enable_sensors(uint8_t sensors[], size_t num_sensors)
{
    if (!bmi270_initialized) {
        ESP_LOGE("BMI270", "BMI270 not initialized");
        return ESP_ERR_INVALID_STATE;
    }

    xSemaphoreTake(imu_mutex,portMAX_DELAY);
    
    int8_t enable_result = bmi270_sensor_enable(sensors, num_sensors, bmi270_handle);
    if (enable_result != 0) {
        ESP_LOGE("BMI270", "Failed to enable sensors");
        return ESP_FAIL;
    }

    memcpy(enable_sensors, sensors, num_sensors);
    enable_sensor_count = num_sensors;

    // 构造需要更新的数据数组
    for(int i =0;i<enable_sensor_count;i++)
    {
        if(enable_sensors[i]==BMI2_ACCEL ||
            enable_sensors[i]==BMI2_GYRO ||
            enable_sensors[i]==BMI2_AUX)
            sensor_update_count++;
        else if(enable_sensors[i]==BMI2_STEP_COUNTER ||
            enable_sensors[i]==BMI2_STEP_ACTIVITY ||
            enable_sensors[i]==BMI2_WRIST_GESTURE ||
            enable_sensors[i]==BMI2_NVM_STATUS ||
            enable_sensors[i]==BMI2_VFRM_STATUS)
        {
            sensor_feature_update[feature_update_count] = enable_sensors[i];
            feature_data[feature_update_count++].type = enable_sensors[i];
        }
        
    }

    xSemaphoreGive(imu_mutex);
    ESP_LOGI("BMI270", "Sensors enabled successfully");
    return ESP_OK;
}

// 禁用传感器
esp_err_t imu_disable_sensors(uint8_t sensors[], size_t num_sensors)
{
    if (!bmi270_initialized) {
        ESP_LOGE("BMI270", "BMI270 not initialized");
        return ESP_ERR_INVALID_STATE;
    }

    int8_t disable_result = bmi270_sensor_disable(sensors, num_sensors, bmi270_handle);
    if (disable_result != 0) {
        ESP_LOGE("BMI270", "Failed to disable sensors");
        return ESP_FAIL;
    }

    ESP_LOGI("BMI270", "Sensors disabled successfully");
    return ESP_OK;
}


// 获取传感器数据
esp_err_t imu_get_sensor_data(void)
{
    if (!bmi270_initialized) {
        ESP_LOGE("BMI270", "BMI270 not initialized");
        return ESP_ERR_INVALID_STATE;
    }

    struct bmi2_sens_data sensor_data;

    bmi2_get_sensor_data(&sensor_data, bmi270_handle);
    
    // int8_t data_result = bmi270_get_feature_data(&accel_feature_data, 1, bmi270_handle);
    // if (data_result != 0) {
    //     ESP_LOGE("BMI270", "Failed to get accel feature data");
    //     return ESP_FAIL;
    // }
    // int8_t data_result = bmi270_get_feature_data(&gyro_feature_data, 1, bmi270_handle);
    // if (data_result != 0) {
    //     ESP_LOGE("BMI270", "Failed to get gyro feature data");
    //     return ESP_FAIL;
    // }

    // ESP_LOGI("BMI270", "accel data: %d,%d,%d", sensor_data.acc.x, sensor_data.acc.y, sensor_data.acc.z);
    ESP_LOGI("BMI270", "gyro data: %d,%d,%d", sensor_data.gyr.x, sensor_data.gyr.y, sensor_data.gyr.z);
    return ESP_OK;
}


